#include "tf_calibration.h"

TfCalibration::TfCalibration(ros::NodeHandle &n)
{
	for(size_t i = 0; i < NUMAXIS; i++)
		mBlobDetector[i] = new BlobDetection(n);
	mFound = false;

	colors[0].colorHue = 108;
	colors[0].colorSpan = 39;
	colors[0].colorSat = 14;
	colors[1].colorHue = 175;
	colors[1].colorSpan = 35;
	colors[1].colorSat = 53;
	colors[2].colorHue = 60;
	colors[2].colorSpan = 27;
	colors[2].colorSat = 50;
}

TfCalibration::~TfCalibration()
{

}

void TfCalibration::ImageCallback(cv::Mat &image, const sensor_msgs::CameraInfoConstPtr &camInfo)
{

	for(size_t i = 0; i < NUMAXIS; i++)
	{
		mBlobDetector[i]->updateModel(camInfo);
		mBlobDetector[i]->setHuecolorSpan(colors[i].colorHue,colors[i].colorSpan, colors[i].colorSat);
		mBlobDetector[i]->detectBlobs(image);

		vector<detectedObject> objects;

		objects = mBlobDetector[i]->GetObjects();

		if(objects.size())
		{
			colors[i].point = objects[0].mObjectPos;
			colors[i].screenPoint = objects[0].screenPos;
			colors[i].found = true;
		}
		else
			colors[i].found = false;
	}

	for(size_t i = 0; i < NUMAXIS; i++)
	{
		if(colors[i].found == true)
			cv::circle(image, colors[i].screenPoint, 4, cv::Scalar(0,255,255));

		cv::circle(image, mFixturePoints[i].point2d, 10, cv::Scalar(0,0,255), 3);
	}
}

void TfCalibration::CalculateTransform()
{
	bool notFound = false;

	for(size_t i = 0; i < NUMAXIS; i++)
	{

		if(colors[i].found == false)
		{
			notFound = true;
		}
	}

	if(notFound == false)
	{
		for(size_t i = 0; i < NUMAXIS; i++)
		{
			mFixturePoints[i].point = colors[i].point;
			mFixturePoints[i].point2d = colors[i].screenPoint;
		}

		// Set offset
		mOffset.x = -0.85;
		mOffset.y = 0.45;
		mOffset.z = 0.75;//mFixturePoints[0].point;


		cv::Point3d baseVectorX = mFixturePoints[1].point - mFixturePoints[0].point;
		cv::Point3d baseVectorY = mFixturePoints[2].point - mFixturePoints[0].point;
		cv::Point3d baseVectorZ = baseVectorX.cross(baseVectorY);


		float baseVectorXlen = sqrt(baseVectorX.dot(baseVectorX));
		float baseVectorYlen = sqrt(baseVectorY.dot(baseVectorY));
		float baseVectorZlen = sqrt(baseVectorZ.dot(baseVectorZ));

		// Projections
		float projX = (baseVectorX.dot(mFixturePoints[0].point) / baseVectorXlen);
		float projY = (baseVectorY.dot(mFixturePoints[0].point) / baseVectorYlen);
		float projZ = (baseVectorZ.dot(mFixturePoints[0].point) / baseVectorZlen);

		mOffset.x = -projX;
		mOffset.y = -projY;
		mOffset.z = -projZ;

                ROS_INFO("ProjX: %f\nProjY: %f\nProjZ: %f", projX, projY, projZ);
		ROS_INFO("baseVectorXlen2: %f\nbaseVectorYlen2: %f\nbaseVectorZlen2: %f", baseVectorXlen, baseVectorYlen, baseVectorZlen);

                cv::Point3d kinectX(1,0,0);
                float kinectProjXX = (kinectX.dot(baseVectorX) / baseVectorXlen);
                float kinectProjXY = (kinectX.dot(baseVectorY) / baseVectorYlen);
//              float kinectProjXZ = (kinectX.dot(baseVectorZ) / baseVectorZlen);
                mRotation.z = atan2(kinectProjXY, kinectProjXX) + M_PI/2; // THIS WORKS (yaw)


                // THIS DOESN'T (pitch)

                cv::Point3d kinectZrot;
                kinectZrot.x = cos(mRotation.z) * kinectX.x; // - sin(mRotation.z) * kinectX.y
                kinectZrot.y = sin(mRotation.z) * kinectX.x; // + cos(mRotation.z) * kinectX.y
                kinectZrot.z = 0;

                cv::Point3d normalizedBaseVectorX = baseVectorX * (1/baseVectorXlen);

//                mRotation.y = atan2(kinectZrot.z - normalizedBaseVectorX.z, kinectZrot.x - normalizedBaseVectorX.x) + M_PI/2;
				mRotation.y = toRad(30);

//                ROS_INFO("ProjX: %f\nProjY: %f\nProjZ: %f", kinectProjZX, kinectProjZY, kinectProjZZ);

		// Calculate rotation between the systems
                mRotation.x = 0; //atan2(kinectProjZ, kinectProjY);

		ROS_INFO("Roll(X): %f\nPitch(Y): %f\nYaw(Z): %f\n", mRotation.x, mRotation.y, mRotation.z);

		/*
		mRotation.x = 0; // ROLL
		mRotation.y = 0.5; // PITCH
		mRotation.z = 0;//atan2(vector2.y, vector2.x); // YAW

			const float roll = -toRad(0);
			const float pitch = 0;
			const float yaw = -toRad(0);
		*/

		mFound = true;
	}
}

tf::StampedTransform TfCalibration::RequestTransform()
{
	if(!mFound)
	{
		tf::Quaternion orientation = tf::createQuaternionFromRPY(0, 0, 0);
		tf::Vector3 translation(0, 0, 0);
		return tf::StampedTransform(tf::Transform(orientation, translation),
									ros::Time::now()+ros::Duration(0.05),"frame_link", "camera_link");
	}

	tf::Quaternion orientation = tf::createQuaternionFromRPY(mRotation.x, mRotation.y, mRotation.z);
	tf::Vector3 translation(mOffset.x, mOffset.y, mOffset.z);


	return tf::StampedTransform(tf::Transform(orientation, translation),
								ros::Time::now()+ros::Duration(0.05),"frame_link", "camera_link");
}
